#include "map_server_2d.h"

#ifdef HAVE_YAMLCPP_GT_0_5_0
// The >> operator disappeared in yaml-cpp 0.5, so this function is
// added to provide support for code written under the yaml-cpp 0.3 API.
template<typename T>
void operator >> (const YAML::Node& node, T& i) {
  i = node.as<T>();
}
#endif

namespace slam_ros{

MapServer2D::MapServer2D(const std::string& map_frame_id, const rclcpp::NodeOptions& options)
  : rclcpp::Node("map_server_2d", options), frame_id_(map_frame_id) {}

bool MapServer2D::loadMapFromYaml(
    nav_msgs::srv::GetMap::Response* map_resp_ptr,  
    const std::string& path_to_yaml) {
  std::string mapfname;
  double res;
  int negate;
  double occ_th;
  double free_th;
  double origin[3];
  try {
    YAML::Node doc = YAML::LoadFile(path_to_yaml);  // 直接加载文件

    res = doc["resolution"].as<double>();
    negate = doc["negate"].as<int>();
    occ_th = doc["occupied_thresh"].as<double>();
    free_th = doc["free_thresh"].as<double>();
    origin[0] = doc["origin"][0].as<double>();
    origin[1] = doc["origin"][1].as<double>();
    origin[2] = doc["origin"][2].as<double>();
    mapfname = doc["image"].as<std::string>();

    // 处理相对路径
    if (mapfname.empty()) {
      RCLCPP_ERROR(this->get_logger(), "Image path is empty");
      return false;
    }
    if (!boost::filesystem::path(mapfname).is_absolute()) {
      mapfname = (boost::filesystem::path(path_to_yaml).parent_path() / mapfname).string();
    }
  } catch (YAML::Exception &e) {
    RCLCPP_ERROR(this->get_logger(), "YAML parsing error: %s", e.what());
    return false;
  }
  return loadMapFromValues(map_resp_ptr, mapfname, res, origin, negate, occ_th, free_th);
}

bool MapServer2D::loadMapFromValues(
    nav_msgs::srv::GetMap::Response* map_resp_ptr,  
    std::string map_file_name,  
    double resolution,
    double origin[3], 
    int negate, 
    double occ_th, 
    double free_th) {

  RCLCPP_INFO(this->get_logger(), "Loading map from image \"%s\"", map_file_name.c_str());
  
  nav2_map_server::LoadParameters load_parameters;
  load_parameters.image_file_name = map_file_name;
  load_parameters.resolution = resolution;
  load_parameters.origin[0] = origin[0];
  load_parameters.origin[1] = origin[1];
  load_parameters.origin[2] = origin[2];
  load_parameters.negate = negate;
  load_parameters.occupied_thresh = occ_th;
  load_parameters.free_thresh = free_th;
  load_parameters.mode = nav2_map_server::MapMode::Trinary;  

  try {
    nav2_map_server::loadMapFromFile(load_parameters, map_resp_ptr->map);
  } catch (std::runtime_error& e) {
    RCLCPP_ERROR(this->get_logger(), "%s", e.what());
    return false;
  }

  auto now = this->now();
  map_resp_ptr->map.info.map_load_time = now;
  map_resp_ptr->map.header.frame_id = frame_id_;
  map_resp_ptr->map.header.stamp = now;
  
  RCLCPP_INFO(this->get_logger(), "Read a %d X %d map @ %.3lf m/cell",
            map_resp_ptr->map.info.width,
            map_resp_ptr->map.info.height,
            map_resp_ptr->map.info.resolution);
  return true;
}

}